
#include "command.h"

command makeCommand (int _commandType ,int _value0 ,int _value1 ,int _value2){

    command retVal;   
    if ((_commandType<0)||(_commandType>UPPERBOUND)||
        (_value0<0)||(_value0>UPPERBOUND)||
        (_value1<0)||(_value1>UPPERBOUND)||
        (_value2<0)||(_value2>UPPERBOUND)) {
        return makeCommand(BROKEN_CMD,  0, 0, 0);
	} else {
        retVal.commandType = _commandType;
        retVal.value[0] = _value0;
        retVal.value[1] = _value1;
        retVal.value[2] = _value2;
        return retVal;
    }
       
}

uint8_t normalize (uint8_t yy){
    return (yy<DELIMITER)?yy:(yy-1);
}

uint8_t denormalize (uint8_t yy){
    return (yy<DELIMITER)?yy:(yy+1);
}


que makeQue(command CZ){
    que retVal;
	uint8_t crcVal;
	uint8_t temp[4];
	
    command C;
    C.commandType = CZ.commandType;
    C.value[0] = denormalize(CZ.value[0]);
    C.value[1] = denormalize(CZ.value[1]);
    C.value[2] = denormalize(CZ.value[2]);
    //C.value[3] = denormalize(CZ.value[3]);
    
    
	makeEmpty(&retVal);
	
	switch (C.commandType){
		case SETTEMP_CMD:
        
			temp[0] = 255;
			temp[1] = C.value[0]; //absis
			temp[2] = C.value[1]; //ordinat
			temp[3] = C.value[2]; //targetTemp
			
			
			enque(&retVal, temp[0]); 
			enque(&retVal, temp[1]);
			enque(&retVal, temp[2]); 
			enque(&retVal, temp[3]);
			
			crcVal = calculateCRC( temp, 4);

			enque(&retVal, crcVal);
			enque(&retVal, DELIMITER);			
			
		break;
		case REPTEMP_CMD:
			temp[0] = 250;
			temp[1] = 255;
			temp[2] = C.value[0]; //ActualTemp
			temp[3] = C.value[1]; //BGTemp
			
			enque(&retVal, temp[0]); 
			enque(&retVal, temp[1]);
			enque(&retVal, temp[2]); 
			enque(&retVal, temp[3]);
			
			crcVal = calculateCRC( temp, 4);
			
			enque(&retVal, crcVal);
			enque(&retVal, DELIMITERREP);	
			
		break;
		case SETPID_CMD: 
			temp[0] = 253;
			temp[1] = C.value[0]; //Kp
			temp[2] = C.value[1]; //Ki
			temp[3] = C.value[2]; //Kd
			
			enque(&retVal, temp[0]); 
			enque(&retVal, temp[1]);
			enque(&retVal, temp[2]); 
			enque(&retVal, temp[3]);;
			
			crcVal = calculateCRC( temp, 4);
			
			enque(&retVal, crcVal);
			enque(&retVal, DELIMITER);	
			
		break;
		case SETID_CMD:
			temp[0] = 245;
			temp[1] = 245;
			temp[2] = C.value[0]; //Absis
			temp[3] = C.value[1]; //Ordinat
			
			enque(&retVal, temp[0]); 
			enque(&retVal, temp[1]);
			enque(&retVal, temp[2]); 
			enque(&retVal, temp[3]);
			
			crcVal = calculateCRC( temp, 4);
			
			enque(&retVal, crcVal);
			enque(&retVal, DELIMITER);	
			
		break;
		case REPID_CMD:
			temp[0] = 240;
			temp[1] = 245;
			temp[2] = C.value[0]; //Absis
			temp[3] = C.value[1]; //Ordinat
			
			enque(&retVal, temp[0]); 
			enque(&retVal, temp[1]);
			enque(&retVal, temp[2]); 
			enque(&retVal, temp[3]);
			
			crcVal = calculateCRC( temp, 4);
			
			enque(&retVal, crcVal);
			enque(&retVal,DELIMITERREP);	
			
		break;	
        default:
            enque(&retVal, 0);
            enque(&retVal, 0);
            enque(&retVal, 0);
            enque(&retVal, 0);
            enque(&retVal, 0);
            enque(&retVal, 0);
    }	
	//printQue(retVal);
	return retVal;	
}

command parseQue (que Q){
	command retVal = makeCommand(BROKEN_CMD,  0, 0, 0);
	
	uint8_t value[4],zvalue[4], crc ,del;
	uint8_t crcVal;
	
	//printQue(Q);
	
	peekReset(&Q);
	
	zvalue[0] = peek(&Q);
	zvalue[1] = peek(&Q);
	zvalue[2] = peek(&Q);
	zvalue[3] = peek(&Q);
    
    value[0] = normalize(zvalue[0]);
    value[1] = normalize(zvalue[1]);
    value[2] = normalize(zvalue[2]);
    value[3] = normalize(zvalue[3]);
    
    
	
	crc    = peek(&Q);
	del = peek(&Q);
	
	
	if ((del == DELIMITER)||(del == DELIMITERREP)) {
		crcVal = calculateCRC(zvalue, 4);
		//printf("crc %d  --> crc' %d\n", crc, crcVal);
		if ( isCRCEqual( crc, crcVal)){ 
			if ((zvalue[0] == 250)&&(zvalue[1] == 255)){
				// REPTEMP_CMD
                retVal = makeCommand(REPTEMP_CMD ,value[2] ,value[3] ,0 );
	
			} else if ((zvalue[0] == 245)&&(zvalue[1] == 245)){
				// SETID_CMD
                retVal = makeCommand(SETID_CMD ,value[2] ,value[3] ,0 );
				
			} else if ((zvalue[0] == 240)&&(zvalue[1] == 245)){
				// REPID_CMD
                retVal = makeCommand(REPID_CMD ,value[2] ,value[3] ,0 );
				
			} else if (zvalue[0] == 253){
				// SETPID_CMD
                retVal = makeCommand(SETPID_CMD ,value[1] ,value[2] ,value[3] );
				
			} else if (zvalue[0] == 255 ){
				// SETTEMP_CMD
                retVal = makeCommand(SETTEMP_CMD ,value[1] ,value[2] ,value[3] );
	
			}
		} else {
            return makeCommand(FALSECRC_CMD, 0, 0, 0);
        }
	} /*else {
        printQue(Q);
        return makeCommand(BROKEN_CMD,  0, 0, 0);
    }*/
	return retVal;
}

uint8_t calculateCRC( const uint8_t *data, uint8_t data_len){
	uint8_t crc = 0, tbl_idx;
	
	/*
	int i;
	for (i=0; i <4; ++i){
		printf("%d ",data[i]);
	}
	printf("\n");
	*/
	
	while (data_len--) {
		tbl_idx = ((crc >> 0) ^ *data) & 0xff;
		crc = (crc_table[tbl_idx] ^ (crc << 8)) & 0xff;
		data++;
	}
    
	crc &= 0xff;
	
	if (crc == 10) {crc = 11;}
	// Delimiter
	
	return crc;
}

int isCRCEqual(uint8_t crc1, uint8_t crc2){
	if (crc1 == crc2) {
		return 1;
	} else {
		return 0;
	}
}

#ifdef PC
void printCommand(command C){
	printf("C:: ");
	
	switch (C.commandType) {
		case SETTEMP_CMD:
			printf("SETTEMP_CMD : (%03d,%03d) TGT: %03d\n", C.value[0], C.value[1], C.value[2]); 
					
		break;
		case REPTEMP_CMD:
			printf("REPTEMP_CMD : ACT: %03d BG:%03d\n", C.value[0], C.value[1]);		
			
		break;
		case SETPID_CMD: 
			printf("SETPID_CMD : Kp: %03d Ki:%03d Kd:%03d\n", C.value[0], C.value[1], C.value[2]);	
			
		break;
		case SETID_CMD:
			printf("SETID_CMD : (%03d,%03d)\n", C.value[0], C.value[1]);	
			
		break;
		case REPID_CMD:
			printf("REPID_CMD : (%03d,%03d)\n", C.value[0], C.value[1]);	
			
		break;	 
		case BROKEN_CMD:
			printf("BROKEN_CMD\n");
		break;
        case FALSECRC_CMD:
            printf("FALSECRC_CMD\n");
        break;
        default:
            printf("ERR\n");
        break;
	}
}



#endif
int isCommandEqual(command C1, command C2){
	int retval = 1,i;
    // printf("------\n");
	// printCommand(C1);
	// printCommand(C2);
	
	if (C1.commandType != C2.commandType){
		retval = 0;
		//printf("different command\n");
	}
	
	for (i=0;i<3;++i){
		if (C1.value[i] != C2.value[i]){
			retval = 0;
			//printf("%d: %d - %d\n",i, C1.value[i], C2.value[i] );
		}
	}
	
	return retval;
}
